# include "rclcpp/rclcpp.hpp"
# include <memory>  // 智能指针相关头文件
# include "tf2_ros/static_transform_broadcaster.h" // 静态坐标广播器头文件
# include "geometry_msgs/msg/transform_stamped.hpp" // 坐标消息头文件
# include "tf2/LinearMath/Quaternion.h"  // 四元数头文件
# include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" // 四元数消息头文件


class StaticTFBroadcaster : public rclcpp::Node
{
    private:
        std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
        
    public:
        StaticTFBroadcaster() : Node("static_tf_broadcaster")
        {
            // 创建静态坐标广播器
            static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
            // 创建坐标消息
            this->publish_tf();
        }
        void publish_tf(){
            geometry_msgs::msg::TransformStamped transform;
            transform.header.stamp = this->get_clock()->now();
            transform.header.frame_id = "map";
            transform.child_frame_id = "target_link";
            transform.transform.translation.x = 5.0;
            transform.transform.translation.y = 3.0;
            transform.transform.translation.z = 0.0;
            tf2::Quaternion q;
            q.setRPY(0.0, 0.0, 60*M_PI/180);
            transform.transform.rotation = tf2::toMsg(q);
            static_tf_broadcaster_->sendTransform(transform);
            RCLCPP_INFO(
            this->get_logger(),
            "Publish static transform from %s to %s: "
            "Translation: (%.2f, %.2f, %.2f) "
            "Rotation: (%.2f, %.2f, %.2f, %.2f)",
            transform.header.frame_id.c_str(),
            transform.child_frame_id.c_str(),
            transform.transform.translation.x,
            transform.transform.translation.y,
            transform.transform.translation.z,
            transform.transform.rotation.x,
            transform.transform.rotation.y,
            transform.transform.rotation.z,
            transform.transform.rotation.w
        );
        }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<StaticTFBroadcaster>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}